/*
 * Copyright (C) 2014, Field and Service Robotics 2014, Group 6.
 *
 *  Motion Ctrl
 */
#ifndef MOTION_CTRL_H_
#define MOTION_CTRL_H_

#include <ctime>

#include "ros/ros.h"

#include "std_msgs/Bool.h"
#include <nav_msgs/Odometry.h>
#include <group6/motion_ctrl.h>
#include <group6/approach.h>
#include <group6/kinectApproach.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>


/* Class for handling approaching movements which are done without sending goals
 * to movement_base.
 *
 * Calling from another node:
 * publish topic "goToPoint" with msg "group6/motion_ctrl",
 * the message includes two double values:
 * first for linear movement in meters (positive forward, negative backward)
 * second for rotational movement in radians (positive counter clockwise, negative clockwise)
 */

class MotionCtrl
{
public:

  enum currentStates {STOPPED = 1, MOVING_FORWARD = 2, SPINNING = 3, EMERGENCY_STOPPED = 4 , APPROACH_INTERRUPTED =5};
  enum movementTypes {STRAIGHT_FORWARD = 1, SPIN_CLOCKWISE = 2, SPIN_COUNTER_CLOCKWISE = 3 };

  //TO STATE MACHINE
  enum status {SUCCEED = 0, NOT_SUCCEED = 1, ERROR_TOY_NOT_FOUND = 2, MAX_TIME_EXCEEDED = 3};

  enum serviceType {APPROACH = 1, ESCAPE_FROM_BASKET = 2, ESCAPE_FROM_TOY = 3, BACKOFF = 4, APPROACH_BASKET1 = 5, APPROACH_BASKET2 = 6};
  enum kinectResponse {KINECT_TOY_NOT_FOUND = 0, KINECT_TOY_FOUND = 1};
  enum approachingState {IN_PROGRESS_LINEAR= 0, IN_PROGRESS_ROTATE= 1 , WAIT_FOR_MESSAGE = 2, IDLE = 3};

  MotionCtrl(ros::NodeHandle& nh);
  ~MotionCtrl(){}

  bool approachService(group6::approach::Request &req, group6::approach::Response &res);

  void obstacleApproaching(const std_msgs::Bool::ConstPtr& boolean);

  void stopMovementCmdReceived(const std_msgs::Bool::ConstPtr &boolean);

  void sendCurrentState(const currentStates& currentState);

  bool driveLinear(double distance, bool approaching);

  bool rotate(double radians, bool approaching);

  void goToPoint(const group6::motion_ctrl::ConstPtr& msg);
  
  void stopMovement(enum currentStates state);


  //setters
  void setVelPublisher(ros::Publisher pub){mVelPublisher = pub;}
  void setStatePublisher(ros::Publisher pub){mStatePublisher = pub;}

private:

    double calculateAngleDifference(const double& current,const double& original);

    bool getCurrentTransform(tf::StampedTransform& currentTransform);
    
    bool checkIfMaxApproachTime();

    status approach();

    status escapeFromBasket();

    status escapeFromToy();

    status backOff();

    status approachBasket(int service);

    double getAngleToBackOffPoint(tf::StampedTransform &currentTransform);

    double mDistance;
    double mAngle;
    ros::ServiceServer mApproachService;
    bool mApproachDone;
    bool mErrorDuringApproach;
    bool mMaxTimeElapsedDuringApproach;
    enum kinectResponse mKinectCurrentResponse;
    enum approachingState mCurrentApproachingState;
    
    //clock_t mStartTime;
    ros::Time mStartTime;


    bool mEmergencyStop;
    bool mApproachInterruptedLinear;
    bool mApproachInterruptedRotate;
    enum currentStates mCurrentState;
    std::pair<double,double> mCurrentBasket;
    enum serviceType mCurrentService;
    
    

    ros::NodeHandle mNodeHandle;
    ros::Publisher mVelPublisher;
    ros::Publisher mStatePublisher;
    tf::TransformListener mListener;
    ros::ServiceClient mSafeToMoveClient;
    //ros::ServiceClient mTriggerKinectClient;
    ros::Publisher mTriggerKinectClient;
    
    group6::kinectApproach mServiceToKinect;
};

#endif
